#ifndef RANSAC_HPP
#define RANSAC_HPP

#include "RandomPermutation.hpp"
#include <thread>

template<typename DATA, typename MODEL>
class Ransac
{
public:
    MODEL model;
protected:
    DATA *data;
    size_t nData;
    size_t nSelect;

    double errorThreshold;
    double acceptPercentage;

    double MissRatioOnce() const
    {
        double ratio=1;
        size_t nDataHit=nData*acceptPercentage;
        for(size_t i=0;i<nSelect;i++)
        {
            ratio*=(nDataHit-i);
            ratio/=(nData-i);
        }
        return 1-ratio;
    }

    double IterateCount(double lgp=-12) const
    {
        return lgp/log10(MissRatioOnce());
    }

    Ransac()
        : data(NULL)
        , nData(0)
        , nSelect(0)
        , errorThreshold(0)
        , acceptPercentage(1)
    {

    }

    void Release()
    {
        if(data!=NULL)
            delete []data;
    }

    virtual ~Ransac()
    {
        Release();
    }


    virtual void ComputeError(double *error, const MODEL &modelFit) const
    {
        memset(error,0,nData*sizeof(*error));
    }

    virtual void ComputeModel(const size_t *indexSelect, MODEL &modelFit) const
    {

    }

    size_t AcceptCount(const double *error) const
    {
        size_t n=0;
        for(size_t i=0;i<nData;i++)
        {
            if(error[i]<errorThreshold)
                n++;
        }
        return n;
    }

    bool FitModel(size_t &acceptCountMax, MODEL &modelFit, size_t **indexSelects, size_t nIterate) const
    {
        double *error=new double[nData];
        acceptCountMax=0;
        for(size_t i=0;i<nIterate;i++)
        {
            MODEL mt;
            ComputeModel(indexSelects[i],mt);
            ComputeError(error,mt);
            size_t ac=AcceptCount(error);
            if(ac>acceptCountMax)
            {
                acceptCountMax=ac;
                modelFit=mt;
				//std::cout << "modelFit=" << modelFit << "\n";
				//std::cout << "acceptCountMax=" << acceptCountMax<<"\n";
                if(acceptCountMax>nData*acceptPercentage)
                {
                    delete []error;
                    return true;
                }
            }
        }
        delete []error;
        return false;
    }

    static void FitModelThread(Ransac *pdata, bool *result, size_t *acceptCountMax, MODEL *modelFit, size_t **indexSelects, size_t nIterate)
    {
        (*result) = pdata->FitModel((*acceptCountMax), (*modelFit), indexSelects, nIterate);
    }

public:

    bool Fit(size_t &acceptCountMax, size_t nThread, size_t nIterate)
    {

        RandomPermutation rp(nIterate);
        nIterate=rp.Generate(nData, nSelect);

        std::cout<<"nIterate="<<nIterate<<'\n'<<std::flush;

        if(nIterate==0)
            return false;

        if(nThread<2)
            return FitModel(acceptCountMax, model, rp.index, nIterate);


        size_t *acm=new size_t[nThread];
        bool *res=new bool[nThread];
        MODEL *mf=new MODEL[nThread];
        std::vector<std::thread> thl;

        size_t nIterateSeg=(nIterate+nThread-1)/nThread;
        size_t start=0;
        size_t i=0;
        while(start<nIterate-nIterateSeg)
        {
            thl.push_back(std::thread(FitModelThread,this,&(res[i]),&(acm[i]),&(mf[i]),&(rp.index[start]),nIterateSeg));
            start+=nIterateSeg;
            i++;
        }

        thl.push_back(std::thread(FitModelThread,this,&(res[i]),&(acm[i]),&(mf[i]),&(rp.index[start]),nIterate-start));

        for(auto& th : thl) th.join();

        size_t maxIndex=std::max_element(acm,acm+nThread)-acm;

		

        acceptCountMax=acm[maxIndex];
        model=mf[maxIndex];
        bool result=res[maxIndex];

        delete []acm;
        delete []res;
        delete []mf;

        return result;
    }


    bool Fit(size_t &acceptCountMax, size_t nThread=16)
    {
        return Fit(acceptCountMax, nThread, IterateCount());
    }

    void GetInOutFlag(bool *inFlag, double *error)
    {
        ComputeError(error, model);
        for(size_t i=0;i<nData;i++)
        {
            inFlag[i]=(error[i]<errorThreshold);
        }
    }

    double EvaluateErrorAll()
    {
        double *error=new double[nData];
        ComputeError(error, model);
        double sum=std::accumulate(error,error+nData,0);
        delete []error;
        return sum/nData;
    }

    size_t RearrangeData(DATA *dataR)
    {
        double *error=new double[nData];
        ComputeError(error, model);
        size_t inIndex=0;
        size_t outIndex=nData-1;
        for(size_t i=0;i<nData;i++)
        {
            if(error[i]<errorThreshold)
            {
                dataR[inIndex]=data[i];
                inIndex++;
            }
            else
            {
                dataR[outIndex]=data[i];
                outIndex--;
            }
        }

        delete []error;
        return inIndex;
    }

};




#endif // RANSAC_HPP
